#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <gazebo_msgs/ModelStates.h>
#include <nav_msgs/Odometry.h>
#include <tf2_ros/transform_broadcaster.h> // 引入 tf2_ros 头文件

#include<iostream>
#include<string>

class gazebo_model_pose
{
private:
    ros::NodeHandle nh_;
    ros::NodeHandle gnh_;
    ros::Publisher model_odom_pub_;
    ros::Publisher model_pose_pub_;
    ros::Subscriber gazebo_modelstates_sub_;

    ros::Timer tf_timer_;
    tf2_ros::TransformBroadcaster tf_broadcaster_; // tf 广播器

    std::string model_name_;
    std::string frame_id_;

public:
    gazebo_model_pose(ros::NodeHandle &nh, ros::NodeHandle &gnh)
    {
        nh_ = nh;
        gnh_ = gnh;
        nh_.getParam("model_name_", model_name_);
        nh_.getParam("frame_id_", frame_id_);
        std::cout << "\033[34m[gazebo_model_pose_pub] Model name is:\033[0m" << model_name_ << "." << std::endl;
        model_odom_pub_ = nh_.advertise<nav_msgs::Odometry>("gazebo_model_odom_topic", 100);
        model_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("gazebo_model_pose_topic", 100);
        gazebo_modelstates_sub_ = nh_.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states", 100, &gazebo_model_pose::RcvModelStatesCallback, this);

        tf_timer_ = nh_.createTimer(ros::Duration(0.1), &gazebo_model_pose::PublishTFCallback, this);
    }    
    ~gazebo_model_pose(){}

    void RcvModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
    {
        nav_msgs::Odometry odom;
        geometry_msgs::PoseStamped pose;
        int model_cnt = msg->name.size();
        // "for" in invert order will be faster
        for (int i = model_cnt-1; i >= 0; i--)
        {
            if (msg->name[i] == model_name_)
            {
                odom.header.stamp = ros::Time::now();
                odom.header.frame_id = frame_id_;
                odom.child_frame_id = "";
                odom.pose.pose = msg->pose[i];
                odom.twist.twist = msg->twist[i];
                model_odom_pub_.publish(odom);

                pose.header.stamp = ros::Time::now();
                pose.header.frame_id = frame_id_;
                pose.pose = msg->pose[i];
                model_pose_pub_.publish(pose);
                break;
            }
        }
    }

    void PublishTFCallback(const ros::TimerEvent &event)
    {
        geometry_msgs::TransformStamped transformStamped;

        // 设置时间戳
        transformStamped.header.stamp = ros::Time::now();
        transformStamped.header.frame_id = frame_id_; // 父坐标系
        transformStamped.child_frame_id = model_name_ + "/base_footprint"; // 子坐标系

        // 设置平移和旋转（假设模型的位姿为零）
        transformStamped.transform.translation.x = 0.0;
        transformStamped.transform.translation.y = 0.0;
        transformStamped.transform.translation.z = 0.0;
        transformStamped.transform.rotation.x = 0.0;
        transformStamped.transform.rotation.y = 0.0;
        transformStamped.transform.rotation.z = 0.0;
        transformStamped.transform.rotation.w = 1.0;

        // 广播 tf
        tf_broadcaster_.sendTransform(transformStamped);
    }

};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "gazebo_model_pose_pub");
    ros::NodeHandle nh("~");
    ros::NodeHandle global_nh("");

    gazebo_model_pose intermediary(nh, global_nh);

    ros::spin();
    // ros::Rate loop_rate(30);
    // while (ros::ok())
    // {
    //     ros::spinOnce();
    //     loop_rate.sleep(); 
    // }

    return 0;

}